#!/usr/bin/env python3
import rospy
import numpy as np
from turtle_pkg.msg import force_msg, velocity_msg, position_msg

class Controller:
    '''
    this is a example controller for the turtle
    ros_handleer: is a callback for the position msg
    controller: the turtle controller node
    '''
    def __init__(self):
        self.node = rospy.init_node("controller")
        self.position_subscriber = rospy.Subscriber("position",position_msg,callback=self.positionCallback)
        self.publisher = rospy.Publisher("force",force_msg,queue_size=1)
        self.positionFlag = False
        self.position = np.array([0.0, 0.0], np.float64)
        self.flag = False

    def positionCallback(self,msg):
        '''
        do something for the exact position
        '''
        global flag
        if(msg.x>3):
            self.flag = True
        rospy.loginfo(msg)
        
        self.positionFlag = True
        self.position[0] = msg.x
        self.position[1] = msg.y

    def controller(self):
        '''
        publish the control msg to the turtle
        '''
        rate = rospy.Rate(30)
        while not rospy.is_shutdown():
            msg = force_msg()
            msg.x = 0
            msg.y = 0
            if(self.flag):
                msg.x = -0.85
                msg.y = 1
            self.publisher.publish(msg)
            rate.sleep()

if __name__ == "__main__":
    c = Controller()
    c.controller()
